#pragma once
#include "rtk_planning/planning_adaptor.h"
#include "auto_msgs/planning_point.h"
#include "auto_msgs/planning.h"
#include<string>
#include <limits>

class RtkPlanningCore
{
    public:
        RtkPlanningCore()=delete;
        RtkPlanningCore(const std::string filename);
        void searchNearstPoint(const std::vector<double> query_vector2d);
        void pathBePublished(int num_of_front_point,int num_of_rear_point);
        PlanningPath add_csv_ros_path_;
    private:
        std::string filename_;
        PlanningPath add_csv_rtk_path_;
        PlanningPath2KD  planning_path2kd_; // The adaptor
        planning_kd_tree_t   index_;
        long unsigned int closet_point_num_ =
                                    std::numeric_limits<int>::max();
        double closet_dist_sqr_ = std::numeric_limits<double>::max();
        
};